/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      FOCTailDect.c
 * @author    Fortiortech  Appliction Team
 * @date      2022-09-13
 * @brief     This file contains  TailDect function used for Motor Control.
 */
#include <MyProject.h>

/*---------------------------------------------------------------------------*/
/*  Name     :   void FocDetect(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 顺逆风参数初始化
    /*---------------------------------------------------------------------------*/
void FocDetectInit(void)
{
    MOE = 0;
    FOC_Init(); // FOC的初始化

    FOC_DKP = DQKP_TailWind;       // 顺逆风的电流环KP
    FOC_DKI = DQKI_TailWind;       // 顺逆风的电流环KI
    FOC_QKP = DQKP_TailWind;       // 顺逆风的电流环KP
    FOC_QKI = DQKI_TailWind;       // 顺逆风的电流环KI
    FOC_EKP = OBSW_KP_GAIN_WIND;   // 顺逆风速度估算的KP
    FOC_EKI = OBSW_KI_GAIN_WIND;   // 顺逆风速度估算的KI
    FOC_OMEKLPF = SPEED_KLPF_WIND; // 顺逆风下的速度滤波系数
    SetBit(FOC_CR1, ANGM);         // U、V、W相输出
    FOC_IDREF = 0;                 // 估算模式
    FOC_IQREF = 0;
    MOE = 1; // 打开MOE
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void FOCCloseLoopStart(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 顺风启动
    /*---------------------------------------------------------------------------*/
void FOCCloseLoopStart(void)
{
    // 配置启动的参数，Omega模式
    FOC_EFREQACC = MOTOR_OMEGA_RAMP_ACC;
    FOC_EFREQMIN = MOTOR_OMEGA_RAMP_MIN;
    FOC_EFREQHOLD = MOTOR_OMEGA_RAMP_END;
    SetBit(FOC_CR1, EFAE); // 估算器强制输出
    ClrBit(FOC_CR1, RFAE); // 禁止强拉
    SetBit(FOC_CR1, ANGM); // 估算模式
    // 电流环的PI和输出限赋值
    FOC_DKP = DKP;
    FOC_DKI = DKI;
    FOC_QKP = QKP;
    FOC_QKI = QKI;
    FOC_DMAX = DOUTMAX;
    FOC_DMIN = DOUTMIN;
    FOC_QMAX = QOUTMAX;
    FOC_QMIN = QOUTMIN;
/*********PLL或SMO**********/
#if (EstimateAlgorithm == SMO)
    // 根据不同转速确启动的ATO_BW值
    FOC_EKP = OBSW_KP_GAIN_RUN3;
    FOC_EKI = OBSW_KI_GAIN_RUN3;
    foc_control.iq_ref = IQ_Start_CURRENT;
    foc_control.State_Count = 800;
#elif (EstimateAlgorithm == PLL)
    foc_control.iq_ref = IQ_RUN_CURRENT;
#endif // end    EstimateAlgorithm
    /*estimate parameter set*/
    FOC_OMEKLPF = SPEED_KLPF;
    FOC_IDREF = ID_RUN_CURRENT;  // D轴启动电流
    FOC_IQREF = foc_control.iq_ref; // Q轴启动电流
    PI3_UKH = foc_control.iq_ref;

    foc_control.control_mode = 0;
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void TailWindDealwith(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 顺风逆风参数配置函数
    /*---------------------------------------------------------------------------*/
void FOCTailWindDealwith(void)
{
    TailWindDealwith();
}

void TailWindDealwith(void)
{

    if (foc_control.speed_filtered < S_Value(500))
    {
        MOE = 0;
        ClrBit(DRV_CR, FOCEN); // 关闭FOC

        McStaSet.SetFlag.PosiCheckSetFlag = 0;
        foc_control.position_check_angle = 0; // 角度赋初值
    }
}
